// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <atomic>
#include <cmath>
#include <functional>
#include <iostream>
#include <iterator>
#include <mutex>
#include <thread>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/rate_limiting.h>
#include <franka/robot.h>

#include "examples_common.h"

namespace {
template <class T, size_t N>  // 重载 << 输出 array
std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
    ostream << "[";
    std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
    std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
    ostream << "]";
    return ostream;
}
}  // anonymous namespace 限制函数为内链接, 避免重定义

/**
 * @example joint_impedance_control.cpp
 * An example showing a joint impedance type control that executes a Cartesian motion in the shape
 * of a circle. The example illustrates how to use the internal inverse kinematics to map a
 * Cartesian trajectory to joint space. The joint space target is tracked by an impedance control
 * that additionally compensates coriolis terms using the libfranka model library. This example
 * also serves to compare commanded vs. measured torques. The results are printed from a separate
 * thread to avoid blocking print functions in the real-time loop.
 * 展示 外部关节空间阻抗控制器 + 笛卡尔空间运动生成器, 执行 yz 平面圆周运动.
 * 这个例子展示了如何使用内部运动学逆解, 将笛卡尔轨迹映射到关节空间,
 * 而关节空间的运动由一个使用 libfranka 模型库额外补偿了科氏力项的关节阻抗控制器来跟踪.
 * 这个例子同时展示了 commanded torques 和 measured torques 的对比, 结果由另一个线程输出,
 * 以避免输出函数阻塞 real-time loop
 */

int main(int argc, char** argv) {
    // Check whether the required arguments were passed.
    if (argc != 2) {
        std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
        return -1;
    }
    // Set and initialize trajectory parameters.  设置圆周轨迹的半径/速度/加速时间/运行时间
    const double radius = 0.05;
    const double vel_max = 0.25;
    const double acceleration_time = 2.0;
    const double run_time = 20.0;
    // Set print rate for comparing commanded vs. measured torques.  设置输出频率
    const double print_rate = 10.0;

    double vel_current = 0.0;
    double angle = 0.0;
    double time = 0.0;

    // Initialize data fields for the print thread. 初始化输出线程的数据
    struct {
        std::mutex mutex;  // 互斥锁
        bool has_data;
        std::array<double, 7> tau_d_last; // 上一次的期望力矩
        franka::RobotState robot_state;
        std::array<double, 7> gravity;
    } print_data{};
    std::atomic_bool running{true}; // 因为会被多线程访问, 所以设为原子变量

    // Start print thread. 启动输出线程
    std::thread print_thread([print_rate, &print_data, &running]() {
        while (running) {
            // Sleep to achieve the desired print rate. 阻塞以控制输出频率
            std::this_thread::sleep_for(
                std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0))));

            // Try to lock data to avoid read write collisions. 尝试加锁, 避免读写冲突
            if (print_data.mutex.try_lock()) {
                if (print_data.has_data) {  // 如果有新数据存在
                    std::array<double, 7> tau_error{};
                    double error_rms(0.0);
                    std::array<double, 7> tau_d_actual{};
                    for (size_t i = 0; i < 7; ++i) {
                        tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i];  // 实际关节控制力矩是: 期望输出力矩 + 补偿重力矩
                        tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i];  // 力矩误差 = 实际关节控制力矩 - 实际测量力矩
                        error_rms += std::pow(tau_error[i], 2.0) / tau_error.size();
                    }
                    error_rms = std::sqrt(error_rms);  // 力矩误差均方差

                    // Print data to console
                    std::cout << "tau_error [Nm]: " << tau_error << std::endl
                            << "tau_commanded [Nm]: " << tau_d_actual << std::endl
                            << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl
                            << "root mean square of tau_error [Nm]: " << error_rms << std::endl
                            << "-----------------------" << std::endl;
                    print_data.has_data = false;  // 已输出, 等待下次写入数据
                }
                print_data.mutex.unlock();
            }
        }
    });

    try {
        // Connect to robot.
        franka::Robot robot(argv[1]);
        setDefaultBehavior(robot);

        // 将机器人复位
        std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
        MotionGenerator motion_generator(0.5, q_goal);
        std::cout << "WARNING: This example will move the robot! "
                << "Please make sure to have the user stop button at hand!" << std::endl
                << "Press Enter to continue..." << std::endl;
        std::cin.ignore();
        robot.control(motion_generator);
        std::cout << "Finished moving to initial joint configuration." << std::endl;

        // Set additional parameters always before the control loop, NEVER in the control loop!
        // Set collision behavior.
        robot.setCollisionBehavior(
            {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
            {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
            {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
            {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});

        // Load the kinematics and dynamics model. 加载运动学和动力学模型
        franka::Model model = robot.loadModel();

        std::array<double, 16> initial_pose;

        // Define callback function to send Cartesian pose goals to get inverse kinematics solved.
        // 定义笛卡尔运动轨迹生成器
        auto cartesian_pose_callback = [=, &time, &vel_current, &running, &angle, &initial_pose](
        const franka::RobotState& robot_state, franka::Duration period) -> franka::CartesianPose {
            // Update time.
            time += period.toSec();

            if (time == 0.0) {
                // Read the initial pose to start the motion from in the first time step.
                initial_pose = robot_state.O_T_EE_c;  // 初始状态
            }

            // Compute Cartesian velocity.
            if (vel_current < vel_max && time < run_time) {  // 加速阶段
                vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
            }
            if (vel_current > 0.0 && time > run_time) {  // 减速阶段
                vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
            }
            vel_current = std::fmax(vel_current, 0.0);  // 限制范围
            vel_current = std::fmin(vel_current, vel_max);

            // Compute new angle for our circular trajectory.
            angle += period.toSec() * vel_current / std::fabs(radius);  // 角度 = 弧长 / 半径
            if (angle > 2 * M_PI) {
                angle -= 2 * M_PI;
            }

            // Compute relative y and z positions of desired pose.
            double delta_y = radius * (1 - std::cos(angle));  // 0 -> 2*radius -> 0
            double delta_z = radius * std::sin(angle);  // 0 -> radius -> -radius -> 0
            franka::CartesianPose pose_desired = initial_pose;
            pose_desired.O_T_EE[13] += delta_y;
            pose_desired.O_T_EE[14] += delta_z;

            // Send desired pose.
            if (time >= run_time + acceleration_time) {
                running = false;
                return franka::MotionFinished(pose_desired);
            }

            return pose_desired;
        };

        // Set gains for the joint impedance control. 为关节阻抗控制设置增益
        // Stiffness
        const std::array<double, 7> k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}};
        // Damping
        const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}};

        // Define callback for the joint torque control loop.
        // 定义外部关节阻抗控制器
        std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
            impedance_control_callback = [&print_data, &model, k_gains, d_gains](
            const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques {
            // Read current coriolis terms from model. 读取科氏力项
            std::array<double, 7> coriolis = model.coriolis(state);

            // Compute torque command from joint impedance control law. 由关节阻抗控制定律计算控制力矩
            // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one
            // time step delay.
            // state.q_d 为运动学反解
            std::array<double, 7> tau_d_calculated;
            for (size_t i = 0; i < 7; i++) {
                tau_d_calculated[i] =
                     k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
            }

            // The following line is only necessary for printing the rate limited torque. As we activated
            // rate limiting for the control loop (activated by default), the torque would anyway be adjusted!
            // 这段代码仅仅是为了输出(经过速率限制后的)控制力矩, 这里显式地调用 limitRate 限制力矩的变化率在接口限制范围内.
            // 实际 robot.control 内部会默认启用 rate limiting, 力矩总会被调整
            // state.tau_J_d 为上一周期不带重力的期望力矩, 也即上一周期控制力矩
            std::array<double, 7> tau_d_rate_limited =
                franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d);

            // Update data to print. 更新输出信息
            if (print_data.mutex.try_lock()) {
                print_data.has_data = true;
                print_data.robot_state = state;
                print_data.tau_d_last = tau_d_rate_limited;
                print_data.gravity = model.gravity(state);
                print_data.mutex.unlock();
            }

            // Send torque command.
            return tau_d_rate_limited;
        };

        // Start real-time control loop. 开始控制
        robot.control(impedance_control_callback, cartesian_pose_callback);

    } catch (const franka::Exception& ex) {
        running = false;
        std::cerr << ex.what() << std::endl;
    }

    if (print_thread.joinable()) {
        print_thread.join();
    }
    return 0;
}
